#include "BspM_Driver.h"
#include "can_moto_protocol.h"
#include "timer.h"
int32_t STALL_FLAG=0;
int32_t stall_timer=0;
extern volatile uint32_t millis;
static void HomingUpdate(TypeDefTMC4361_RegStruct *handle)
{
	
	uint32_t TempDat = 0;
//	if((handle->PROTO_STATUS&M_STATUS_HOME_FINISH) == 0)
	if(handle->HOMING_FLAG == 0)
	{
		switch(handle->HOMEING_CMD)
		{
			case TYPE2_FAST_REACH:
				stall_timer=0;
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0);  // 清空限位
						handle->bsp_write_reg(0X01,STOPL_EN | STOPL_L_EN | 0X00000010);  // 使能左限位有效  使能低电平有效停止
						handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
						handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
						delay_ms(1);
//						vTaskDelay(1);
						
						handle->HOMEING_CMD = TYPE2_FAST_LEAVE;
					}
				}
			  break;
			case TYPE2_FAST_LEAVE:
				stall_timer=0;
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
				{
					handle->bsp_write_reg(0X24,0);       // VMAX
					handle->bsp_write_reg(0X01,0);  // 清空限位
					handle->bsp_write_reg(0X01,STOPR_EN | STOPR_H_EN | STOPR_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
					handle->bsp_write_reg(0X24,SLOW_HOME_SPEED);       // 启动电机右转
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					delay_ms(1);
//					vTaskDelay(1);
				//					 while(TempDat == 0)
				//						{
				//							handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				//						}
					handle->HOMEING_CMD = TYPE2_SLOW_REACH;
				}
			  break;
			case TYPE2_SLOW_REACH:
				stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0);  // 清空限位
						handle->bsp_read_reg(0X21,&TempDat);  // 读取实际位置
						handle->bsp_write_reg(0X37,TempDat);   // 目标位置等于实际位置
						delay_ms(2);
//						vTaskDelay(2);
						handle->bsp_write_reg(0X20,0X06);      //     位置模式
						handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG(handle->PROTO_V));       // VMAX	
						handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
						handle->X_LATCH = (int32_t)TempDat;
						handle->XTARGET = handle->X_LATCH;
						handle->HOMEING_CMD = 0;
						handle->HOMING_FLAG = 1;
//						handle->PROTO_STATUS=(handle->PROTO_STATUS|M_STATUS_HOME_FINISH);
						/*回原完成后记录当前编码器位置 方便后面读取功能*/
						handle->SPI_Status = handle->bsp_read_reg(0X50,&TempDat);
						handle->ENC_POS = (int32_t)TempDat;
						handle->ENC_HOME_POS = handle->ENC_POS;
					}
				}
			  break;
			case TYPE4_FAST_REACH:
				stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X01,0);  // 清空限位
						handle->bsp_write_reg(0X01,STOPR_EN | STOPR_L_EN | 0X00000010);  // 使能右限位有效  使能低电平有效停止
						handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
						handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
						delay_ms(1);
//						vTaskDelay(1);
						
						handle->HOMEING_CMD = TYPE4_FAST_LEAVE;
					}
				}
			  break;
			case TYPE4_FAST_LEAVE:
				stall_timer=0;
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
				{
					handle->bsp_write_reg(0X24,0);       // VMAX
					handle->bsp_write_reg(0X01,0);  // 清空限位
					handle->bsp_write_reg(0X01,STOPL_EN | STOPL_H_EN | STOPL_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
					handle->bsp_write_reg(0X24,(int32_t)(-SLOW_HOME_SPEED));       // 启动电机右转
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					delay_ms(1);
				//					 while(TempDat == 0)
				//						{
				//							handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				//						}
					handle->HOMEING_CMD = TYPE4_SLOW_REACH;
				}
			  break;
			case TYPE4_SLOW_REACH:
				stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0);  // 清空限位
						handle->bsp_read_reg(0X21,&TempDat);  // 读取实际位置
						handle->bsp_write_reg(0X37,TempDat);   // 目标位置等于实际位置
						delay_ms(2);
//						vTaskDelay(2);
						handle->bsp_write_reg(0X20,0X06);      //     位置模式
						handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG(handle->PROTO_V));       // VMAX
						handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
						handle->X_LATCH = (int32_t)TempDat;
						handle->XTARGET = handle->X_LATCH;
						handle->HOMEING_CMD = 0;
						handle->HOMING_FLAG = 1;
						
						/*回原完成后记录当前编码器位置 方便后面读取功能*/
						handle->SPI_Status = handle->bsp_read_reg(0X50,&TempDat);
						handle->ENC_POS = (int32_t)TempDat;
						handle->ENC_HOME_POS = handle->ENC_POS;
//						handle->PROTO_STATUS=(handle->PROTO_STATUS|M_STATUS_HOME_FINISH);
					}
				}	
			  break;
			default:stall_timer=0;break;
		}
	}
}
 void motor_poll(TypeDefTMC4361_RegStruct* handle)
{
	uint32_t TempDat = 0;

	/*M1寄存器轮询更新*/
	HomingUpdate(handle);
	
	handle->SPI_Status = handle->bsp_read_reg(0X50,&TempDat);
	handle->ENC_POS = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X52,&TempDat);
	handle->ENC_POS_DEV = (int32_t)TempDat;

	handle->SPI_Status = handle->bsp_read_reg(0X59,&TempDat);
	handle->CL_OFFSET = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X21,&TempDat);
	handle->XACTUAL = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X37,&TempDat);
	
	handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
	handle->X_LATCH = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X0E,&TempDat);
	handle->EVENTS = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	handle->STATUS = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X01,&TempDat);
	handle->REFERENCE_CONF = (int32_t)TempDat;
	
	handle->SPI_Status = handle->bsp_read_reg(0X65,&TempDat);
	handle->V_ENC = (int32_t)TempDat;
	
	
	handle->SPI_Status = handle->bsp_read_reg(0X00,&TempDat); /*偶尔会出现该参数为0 导致电机加速度变为极低*/
	if(TempDat != 0x6026)
	{
		handle->bsp_write_reg(0X00,0x6026);
	}
#if MOTOR_OPENLOOP
	if((ACTUAL_EQ_TAR & handle->STATUS))
	{
		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
		stall_timer=0;
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
	}
	/*速度停止*/
	//if(handle->V_ENC == 0)
	if((handle->STATUS & (VACTURAL_POSITIVE|VACTURAL_NEGETIVE)) == 0)
	{
		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STOP);
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
	}
#else
	/*堵转检测*/
//	if(handle->V_ENC==0&&((ACTUAL_EQ_TAR & handle->STATUS)==0)&&(handle->STATUS &(ENC_OVER_TOL)))
//	{
//		stall_timer=stall_timer+1;
//		if((stall_timer)>=800)
//		{
//		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STALL);
//		if(STALL_FLAG==0){
//		handle->bsp_write_reg(0x37,handle->ENC_POS);
//		STALL_FLAG=1;
//		}
//		}
//	}
#endif
	handle->SPI_Status=handle->bsp_read_reg(0X22,&TempDat);
	handle->VActual=abs((int32_t)TempDat);
	
	handle->PROTO_XACTURAL = handle->XACTUAL - handle->X_LATCH;
#if MOTOR_OPENLOOP
#else
	if((ACTUAL_EQ_TAR & handle->STATUS)&&handle->V_ENC==0)//&&abs(handle->ENC_POS - handle->XTARGET + handle->CL_OFFSET)<=500)
	{
		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
		stall_timer=0;
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
	}
	/*速度停止*/
	//if(handle->V_ENC == 0)
	if((handle->STATUS & (VACTURAL_POSITIVE|VACTURAL_NEGETIVE)) == 0&&handle->V_ENC==0)
	{
		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STOP);
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
	}
	
/*
电机复位时朝光电反方向堵死	读出来的电机运动方向和编码器方向都是朝向光电位置的，实际上电机却反方向堵死了。
电机芯片初始化时在机械限位两端来回碰撞	没有发送运动指令，读出电机编码器的值正确的指示了电机的位置是在来回运动
*/
	/*通过检测编码器寄存器防止碰撞发生*/
	if( abs(handle->ENC_POS - handle->XACTUAL) > 10000 
		|| (ENC_OVER_TOL & handle->STATUS) != 0 /*电机来回运动堵死 编码器读数异常时这个错误位会置位*/
//		|| (handle->STATUS & (STALL_DECTED | STALLGUARD)) != 0 /*出现电机在复位后运动速度变慢且报堵转错误*/
	)
	{
		if(handle->stall_tick == 0)
		{
			__disable_irq();
			handle->stall_tick = millis;
			__enable_irq();
		}
		else
		{
			uint32_t cur_tick;
			__disable_irq();
			cur_tick = millis;
			__enable_irq();
			if((cur_tick - handle->stall_tick) > 3000) /*持续三秒错误则上传错误*/
			{
//				motor_current(handle,0);
//				handle->bsp_write_reg(0X6D,0Xec);      // set CHOPCONF reg to initial value,release motor
//				handle->bsp_write_reg(0X6C,0x10410150); 
//				handle->bsp_nfreeze(1);
				M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STALL);
//				M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_ERR);
			}
		}
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STALL);
		handle->stall_tick = 0;
	}
	
	if(handle->PROTO_STATUS & M_STATUS_HOME_FINISH)
	{
		/*开始运动*/
		if( (handle->PROTO_STATUS & M_STATUS_IN_POSITION) == 0 
			&& handle->pos_status == POS_REACHED
		)
		{
			handle->pos_status = POS_RUNNING;
			__disable_irq();
			handle->start_tick = millis;
			__enable_irq();
			
		}
		/*运动停止了*/
		else if( (handle->PROTO_STATUS & M_STATUS_IN_POSITION) != 0 
			&& handle->pos_status == POS_RUNNING
		)
		{
			__disable_irq();
			handle->stop_tick = millis;
			__enable_irq();
			handle->pos_status = POS_REACHED;
			/*判断是否错误*/
			/*短距离加减速过长 不判断*/
			if(abs(handle->last_relative) < (51200 / 5))
			{
			}
			else
			{
				/*平均速度 单位步数每秒*/
				uint32_t avg_speed = 1000 * abs(handle->last_relative) / abs(handle->stop_tick - handle->start_tick);
				handle->SPI_Status = handle->bsp_read_reg(0X24,&TempDat);
				/*上位机将转每分钟 * 895 发送到下位机 下位机*256后写入0x24寄存器 */
				//TempDat = TempDat / 256 / 895 * 51200 / 60;/*转单位 步数每秒*/
				TempDat = TempDat / 895 / 60 * 200 ;
				if(avg_speed < (TempDat / 10))
				{
					M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_SPEED_ERR);
//					M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_ERR);
				}
			}
			
		}
	}
#endif
	
}
void motor_home(TypeDefTMC4361_RegStruct* handle)
{
	uint32_t TempDat;
	if(handle->HOMEING_CMD == 0)
		return;
	if(handle->X_RANGE != 0)
	{
//		M_Init();
		handle->bsp_read_reg(0X21,&TempDat);
		handle->bsp_write_reg(0X37,TempDat);
//		handle->bsp_write_reg(0X36,0);
		handle->bsp_write_reg(0X01,0);
	}
	switch(handle->HOMEING_CMD)
	{
		case 0X01 : {  //  电机停止回原点
			
		}break;
		case 0X02 : {  //  电机向右旋转回原点高电平停止电机
			handle->bsp_write_reg(0X24,0);       // VMAX
			handle->bsp_write_reg(0X20,0X00);       //     速度模式
			handle->bsp_write_reg(0X01,0);  // 清空限位
			handle->bsp_write_reg(0X01,STOPR_EN | STOPR_H_EN | STOPR_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
			handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	    handle->STATUS= TempDat;
			if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
			{
				handle->bsp_write_reg(0X01,0);  // 清空限位
				handle->bsp_write_reg(0X01,STOPL_EN | STOPL_L_EN | 0X00000010);  // 使能左限位有效  使能低电平有效停止
				handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				delay_ms(1);
//				vTaskDelay(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE2_FAST_LEAVE;//06
			}
			else    // 判断光电为低电平，直接复位
			{
				handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				delay_ms(1);
//				vTaskDelay(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE2_FAST_REACH;//20
			}
		}break;
		case 0X03 : {  //  电机向右旋转回原点低电平停止电机

		}break;
		case 0X04 : {  //  电机向左旋转回原点高电平停止电机
			handle->bsp_write_reg(0X24,0);       // VMAX
			handle->bsp_write_reg(0X20,0X00);       //     速度模式
			handle->bsp_write_reg(0X01,0);  // 清空限位
			handle->bsp_write_reg(0X01,STOPL_EN | STOPL_H_EN | STOPL_E_XLATCH);  // 使能左限位有效  使能高电平有效停止
			handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	    handle->STATUS= TempDat;
			if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
			{
				handle->bsp_write_reg(0X01,0);  // 清空限位
				handle->bsp_write_reg(0X01,STOPR_EN | STOPR_L_EN | 0X00000010);  // 使能右限位有效  使能低电平有效停止
				handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				delay_ms(1);
//				vTaskDelay(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE4_FAST_LEAVE;//08
			}
			else    // 判断光电为低电平，直接复位
			{
				handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				delay_ms(1);
//				vTaskDelay(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE4_FAST_REACH;//30
			}			
		}break;
		case 0X05 : {  //  电机向左旋转回原点低电平停止电机
			
		}break;
		default : break;
	}
}
	
void motor_tarpos(TypeDefTMC4361_RegStruct* handle,int32_t tar)
{
	int32_t Xc;
	int32_t D;
	/*目标位置等于当前位置*/
//	if(handle->PROTO_XTATGET == tar)
//		return;
	handle->last_relative = tar - handle->PROTO_XTATGET;

	handle->PROTO_XTATGET = tar;
	handle->XTARGET = handle->X_LATCH + tar;

if(handle->X_RANGE>0&&handle->HOMING_FLAG==1)
{
		if(handle->XTARGET<0)
	{
		handle->XTARGET=handle->XTARGET%(2*handle->X_RANGE);
		handle->XTARGET=handle->XTARGET+(2*handle->X_RANGE);
	}
	if(handle->XTARGET>(2*handle->X_RANGE))
	{
		handle->XTARGET=handle->XTARGET%(2*handle->X_RANGE);
	}
  Xc=handle->XACTUAL%(2*handle->X_RANGE);
	if(Xc<0)
	{
		Xc+=2*handle->X_RANGE;
	}
		D=handle->XTARGET-Xc;
		if(-handle->X_RANGE<=D<=handle->X_RANGE)
		{
			handle->XTARGET=D+handle->XACTUAL;
		}
		if(D<-handle->X_RANGE)
		{
			handle->XTARGET=(2*handle->X_RANGE+D)+handle->XACTUAL;
		}
		if(D>handle->X_RANGE)
		{
			handle->XTARGET=-(2*handle->X_RANGE-D)+handle->XACTUAL;
		}
	}
	
//	__disable_irq();
//	uint32_t TempDat;
	/*目标位置等于当前位置*/
//	if(handle->PROTO_XTATGET == tar)
//		return;
//	handle->PROTO_XTATGET = tar;
//	handle->XTARGET = handle->X_LATCH + tar;
//	if (handle->X_RANGE>0&&handle->XTARGET!=0)
//		{
//			handle->XTARGET = (handle->X_LATCH + tar)%(2*(handle->X_RANGE));
//				if ((handle->X_RANGE)<handle->XTARGET)
//			{
//				handle->XTARGET=-(2*(handle->X_RANGE))+(handle->XTARGET);
//			}
//			if (-(handle->X_RANGE)>=handle->XTARGET)
//			{handle->XTARGET=(2*(handle->X_RANGE))+(handle->XTARGET);}
//			
//}
	handle->bsp_write_reg(0X37,handle->XTARGET);
	int32_t readout = 0;
	handle->bsp_read_reg(0X37,&readout);
	if(readout != handle->XTARGET)
	{
		handle->bsp_write_reg(0X37,handle->XTARGET);
	}
	delay_ms(1);
//		uint32_t CheckX;
//	handle->bsp_read_reg(0X37,&CheckX);
//	if(CheckX!=handle->XTARGET)
//	{
//		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STALL);
//		M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_SPI_error);
//	}
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
//	__enable_irq();
}
void motor_current(TypeDefTMC4361_RegStruct* handle,int32_t data)
{
int32_t TempDat = data;
    
   int32_t I_Run = (TempDat & 0x0000001F)<<8;//I_RUN(12-8)
    
    
    int32_t I_hold = ((TempDat & 0x001F0000)>>16);//I_HOLD(4-0)
 
  int32_t I_holddelay=0x7<<16;//IHOLDDELAY(19-16)
 
  int32_t current_data=I_holddelay+I_Run+I_hold;
 
  handle->bsp_write_reg(0X6D,0X90);      // IHOLD_IRUN
  handle->bsp_write_reg(0X6C,current_data); 
	  
}
void my_curve(TypeDefTMC4361_RegStruct* handle,int32_t data)
{
//	if (data==1){
//	handle->bsp_write_reg(0X28,30000);     // amax  每平方秒的脉冲
//	handle->bsp_write_reg(0X29,30000);     // dmax  每平方秒的脉冲
//	handle->bsp_write_reg(0X2D,0X300);     // bow1  每立方秒的脉冲
//	handle->bsp_write_reg(0X2E,0X300);     // bow2  每立方秒的脉冲
//	handle->bsp_write_reg(0X2F,0X300);     // bow3  每立方秒的脉冲
//	handle->bsp_write_reg(0X30,0X300);     // bow4  每立方秒的脉冲
//	}
//	if (data==2){
//	handle->bsp_write_reg(0X28,15000);     // amax  每平方秒的脉冲
//	handle->bsp_write_reg(0X29,15000);     // dmax  每平方秒的脉冲
//	handle->bsp_write_reg(0X2D,0X100);     // bow1  每立方秒的脉冲
//	handle->bsp_write_reg(0X2E,0X150);     // bow2  每立方秒的脉冲
//	handle->bsp_write_reg(0X2F,0X150);     // bow3  每立方秒的脉冲
//	handle->bsp_write_reg(0X30,0X100);     // bow4  每立方秒的脉冲
//	}
//	if (data==3){
//	handle->bsp_write_reg(0X28,9000);     // amax  每平方秒的脉冲
//	handle->bsp_write_reg(0X29,9000);     // dmax  每平方秒的脉冲
//	handle->bsp_write_reg(0X2D,0X100);     // bow1  每立方秒的脉冲
//	handle->bsp_write_reg(0X2E,0X150);     // bow2  每立方秒的脉冲
//	handle->bsp_write_reg(0X2F,0X150);     // bow3  每立方秒的脉冲
//	handle->bsp_write_reg(0X30,0X100);     // bow4  每立方秒的脉冲
//	}
	if(data>=0&&data<=20){
	int32_t Amax= 3000*data;
	int32_t Dmax= 3000*data;
	int32_t bow1= 76*data;
	int32_t bow2= 76*data;
	int32_t bow3= 76*data;
	int32_t bow4= 76*data;
	handle->bsp_write_reg(0X28,Amax);     // amax  每平方秒的脉冲
	handle->bsp_write_reg(0X29,Dmax);     // dmax  每平方秒的脉冲
	handle->bsp_write_reg(0X2D,bow1);     // bow1  每立方秒的脉冲
	handle->bsp_write_reg(0X2E,bow2);     // bow2  每立方秒的脉冲
	handle->bsp_write_reg(0X2F,bow3);     // bow3  每立方秒的脉冲
	handle->bsp_write_reg(0X30,bow4);     // bow4  每立方秒的脉冲
	}
	else{
		handle->bsp_write_reg(0X28,30000);     // amax  每平方秒的脉冲
	handle->bsp_write_reg(0X29,30000);     // dmax  每平方秒的脉冲
	handle->bsp_write_reg(0X2D,0X300);     // bow1  每立方秒的脉冲
	handle->bsp_write_reg(0X2E,0X300);     // bow2  每立方秒的脉冲
	handle->bsp_write_reg(0X2F,0X300);     // bow3  每立方秒的脉冲
	handle->bsp_write_reg(0X30,0X300);     // bow4  每立方秒的脉冲
	}
}
